#!/bin/bash

# PX4 AUX4 功能编译脚本
# 这个脚本用于编译包含AUX4功能的PX4固件

set -e

# 颜色定义
RED='\033[0;31m'
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
BLUE='\033[0;34m'
NC='\033[0m' # No Color

# 打印带颜色的消息
print_info() {
    echo -e "${BLUE}[INFO]${NC} $1"
}

print_success() {
    echo -e "${GREEN}[SUCCESS]${NC} $1"
}

print_warning() {
    echo -e "${YELLOW}[WARNING]${NC} $1"
}

print_error() {
    echo -e "${RED}[ERROR]${NC} $1"
}

# 显示帮助信息
show_help() {
    echo "PX4 AUX4 功能编译脚本"
    echo ""
    echo "用法: $0 [选项]"
    echo ""
    echo "选项:"
    echo "  -t, --target TARGET    编译目标 (默认: px4_sitl_default)"
    echo "  -c, --clean            清理编译目录"
    echo "  -u, --upload           编译后自动上传到飞控"
    echo "  -h, --help             显示此帮助信息"
    echo ""
    echo "编译目标示例:"
    echo "  px4_sitl_default       仿真环境"
    echo "  px4_fmu-v5_default     FMU-V5飞控"
    echo "  px4_fmu-v6x_default    FMU-V6X飞控"
    echo ""
    echo "示例:"
    echo "  $0 -t px4_sitl_default"
    echo "  $0 -t px4_fmu-v5_default -u"
    echo "  $0 -c -t px4_fmu-v6x_default"
}

# 检查依赖
check_dependencies() {
    print_info "检查编译依赖..."

    # 检查必要的工具
    local missing_tools=()

    if ! command -v make &> /dev/null; then
        missing_tools+=("make")
    fi

    if ! command -v gcc &> /dev/null; then
        missing_tools+=("gcc")
    fi

    if ! command -v g++ &> /dev/null; then
        missing_tools+=("g++")
    fi

    if ! command -v cmake &> /dev/null; then
        missing_tools+=("cmake")
    fi

    if [ ${#missing_tools[@]} -ne 0 ]; then
        print_error "缺少必要的编译工具: ${missing_tools[*]}"
        print_info "请安装缺失的工具后重试"
        exit 1
    fi

    print_success "编译依赖检查完成"
}

# 清理编译目录
clean_build() {
    print_info "清理编译目录..."

    if [ -d "build" ]; then
        rm -rf build
        print_success "编译目录已清理"
    else
        print_warning "编译目录不存在，跳过清理"
    fi
}

# 编译固件
build_firmware() {
    local target=$1
    local upload=$2

    print_info "开始编译固件: $target"

    # 创建编译目录
    mkdir -p build

    # 编译
    print_info "执行编译命令: make $target"
    if make $target -j$(nproc); then
        print_success "编译成功完成"

        # 如果指定了上传选项
        if [ "$upload" = true ]; then
            print_info "准备上传固件到飞控..."
            if make $target upload; then
                print_success "固件上传成功"
            else
                print_error "固件上传失败"
                exit 1
            fi
        fi

    else
        print_error "编译失败"
        exit 1
    fi
}

# 验证AUX4功能
verify_aux4_functionality() {
    print_info "验证AUX4功能..."

    # 检查关键文件是否存在
    local key_files=(
        "src/modules/mavlink/mavlink_receiver.cpp"
        "msg/ManualControlSetpoint.msg"
        "src/modules/rc_update/params.c"
        "Tools/test_aux4.py"
    )

    for file in "${key_files[@]}"; do
        if [ -f "$file" ]; then
            print_success "✓ $file 存在"
        else
            print_error "✗ $file 不存在"
        fi
    done

    # 检查AUX4相关代码
    if grep -q "aux4" src/modules/mavlink/mavlink_receiver.cpp; then
        print_success "✓ AUX4处理代码已集成"
    else
        print_error "✗ AUX4处理代码未找到"
    fi

    if grep -q "RC_MAP_AUX4" src/modules/rc_update/params.c; then
        print_success "✓ AUX4参数已定义"
    else
        print_error "✗ AUX4参数未找到"
    fi
}

# 主函数
main() {
    local target="px4_sitl_default"
    local clean=false
    local upload=false

    # 解析命令行参数
    while [[ $# -gt 0 ]]; do
        case $1 in
            -t|--target)
                target="$2"
                shift 2
                ;;
            -c|--clean)
                clean=true
                shift
                ;;
            -u|--upload)
                upload=true
                shift
                ;;
            -h|--help)
                show_help
                exit 0
                ;;
            *)
                print_error "未知选项: $1"
                show_help
                exit 1
                ;;
        esac
    done

    print_info "PX4 AUX4 功能编译脚本"
    print_info "编译目标: $target"
    print_info "清理编译: $clean"
    print_info "自动上传: $upload"
    echo ""

    # 检查依赖
    check_dependencies

    # 验证AUX4功能
    verify_aux4_functionality
    echo ""

    # 清理编译目录（如果指定）
    if [ "$clean" = true ]; then
        clean_build
        echo ""
    fi

    # 编译固件
    build_firmware "$target" "$upload"

    print_success "编译过程完成！"
    echo ""
    print_info "下一步操作:"
    echo "  1. 连接QGroundControl到飞控"
    echo "  2. 设置RC_MAP_AUX4参数"
    echo "  3. 校准AUX4通道"
    echo "  4. 运行测试脚本: python3 Tools/test_aux4.py"
}

# 运行主函数
main "$@"
